﻿using lctdevice;
using NDK.Motion.MachineResources;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using static lctdevice.ecat_motion;
using static NDK.Motion.MachineManager.M60BufIos;

namespace NDK.Motion.MachineManager
{
    /// <summary> M60 连续插补运动 </summary>
    internal class M60InterpolationMotion
    {
        private readonly short _cardId = 0;
        private readonly short _fifo = 0;
        short _crd;
        short _dimension;
        private CrdBlockData _crdBlockData = new ecat_motion.CrdBlockData();

        public M60InterpolationMotion()
        {

        }

        public void Initial(short crd, params M60InterpolationMotionParamter[] paramters)
        {
            _crd = crd;

            var axisInfoMap1 = paramters.Where(x => x.Type == M60InterpolationMotionParamterType.Line)
                                 .Select(x => (x as M60Line).Position)
                                 .SelectMany(x => x.AxisPriority).ToList();

            var axisInfoMap2 = paramters.Where(x => x.Type == M60InterpolationMotionParamterType.Line)
                          .Select(x => (x as M60Line).Position)
                          .SelectMany(x => x.AxisList).ToList();


            var axisInfoMap = paramters.Where(x => x.Type == M60InterpolationMotionParamterType.Line)
                                 .Select(x => (x as M60Line).Position)
                                 .SelectMany(x => x.AxisList)
                                 .Distinct()
                                 .Select(x => new
                                 {
                                     x.AxisID,
                                     x.Ratio,
                                 })
                                 .ToDictionary(k => k.AxisID, v => v);

            if (paramters.Length == 0)
            {
                throw new ArgumentException($"传入的轴数量 {axisInfoMap.Count} 不能大于当前插补维度 {_dimension}。");
            }

            Dictionary<int, string> keyValues = new Dictionary<int, string>();
            for (int i = 0; i < axisInfoMap1.Count; i++)
            {
                if (i < axisInfoMap.Count)
                {
                    keyValues.Add(axisInfoMap2[i].AxisID, axisInfoMap1[i].ToString());
                }
            }

            foreach (var item in keyValues.Keys)
            {
                if (axisInfoMap.ContainsKey(item) && keyValues[item] != "1")
                    axisInfoMap.Remove(item);
            }


            _dimension = (short)axisInfoMap.Count;
            if (axisInfoMap.Count < 2)
            {
                throw new ArgumentException($"传入的轴数量 {axisInfoMap.Count} 不能大于当前插补维度 {_dimension}。");
            }




            var axisIdArray = axisInfoMap.Keys.ToArray();
            var axisArray = new short[8];
            for (int i = 0; i < axisIdArray.Length; i++)
            {
                axisArray[i] = (short)axisIdArray[i];
            }
            var originPosArray = new int[8];

            var crdCfg_1 = new ecat_motion.CrdCfg();
            crdCfg_1.dimension = _dimension;
            crdCfg_1.axis = axisArray;
            crdCfg_1.orignPos = originPosArray;
            crdCfg_1.evenTime = 1;           //最小匀速时间：使每段插补能够强制匀速运动的最小时间，若速度曲线只有加速度段和减速段，可以在尖峰处形成匀速的水平直线，减小速度突变带来的冲击，单位cycletime（默认 1ms）
            crdCfg_1.synVelMax = 1_500_000;      //坐标系的最大限制插补速度：分段速度大于该速度会默认按该速度运动，单位 pulse/S
            crdCfg_1.synAccMax = 15_000_000;     //坐标系的最大限制插补加速度：分段加速度大于该加速度会默认按该加速度运动，单位 pulse/S/S                                                                                                      
            crdCfg_1.synDecSmooth = 15_000_000;  //插补平滑停止减速度：单位 pulse/S/S
            crdCfg_1.synDecAbrupt = 15_0000_000; //插补紧急停止减速度：单位 pulse/S/S

            var ret = ecat_motion.M_SetCrd(_crd, ref crdCfg_1, _cardId); //设置连续插补运动参数
            if (ret != 0)
            {
                throw new InvalidProgramException($"卡 {_cardId} 设置插补坐标系 {_crd} 失败，返回值 {ret}。");
            }

            ret = ecat_motion.M_CrdClear(_crd, 1, _fifo, _cardId);
            if (ret != 0)
            {
                throw new InvalidProgramException($"卡 {_cardId} 清空插补坐标系 {_crd} FIFO{_fifo}# 失败，返回值 {ret}。");
            }

            // 速度前瞻功能
            ret = ecat_motion.M_SetVelPlanning(_crd, _fifo, (short)10, crdCfg_1.synAccMax, 100, ref _crdBlockData, _cardId);

            foreach (var item in paramters)
            {
                item.AddToFifo(crdCfg_1, _crd, _fifo, _cardId);
            }
        }

        public bool Move()
        {
            var ret1 = lctdevice.ecat_motion.M_CrdData(_crd, ref _crdBlockData, _fifo, _cardId);

            var ret = ecat_motion.M_CrdStartSingle(_crd, _fifo, _cardId);
            if (ret != 0)
            {
                throw new InvalidProgramException($"卡 {_cardId} 插补坐标系 {_crd} FIFO{_fifo}# 开始插补运动失败，返回值 {ret}。");
            }

            short Sts_1;
            short CmdNum_1;
            int Space_1;
            do
            {
                Thread.Sleep(10);
                ret = ecat_motion.M_CrdStatus(_crd, out Sts_1, out CmdNum_1, out Space_1, _fifo, _cardId);
                if (ret != 0)
                {
                    return false;
                }
            }
            while (Sts_1 == 1 || CmdNum_1 != 0);


            short mask_crd_1 = (short)(1 << _crd - 1);
            short option = 0;    //option 是对应的停止方法，0：平滑停止，1：急停
            ret = ecat_motion.M_CrdStop(mask_crd_1, option, _cardId);
            if (ret != 0)
            {
                throw new InvalidProgramException($"卡 {_cardId} 插补坐标系 {_crd} FIFO{_fifo}# 停止插补运动失败，返回值 {ret}。");
            }

            return true;
        }
    }

    public enum M60InterpolationMotionParamterType
    {
        Line = 0,
        BufIO = 1,
        Delay
    }

    internal abstract class M60InterpolationMotionParamter
    {
        public M60InterpolationMotionParamterType Type { get; private set; }

        protected M60InterpolationMotionParamter(M60InterpolationMotionParamterType type)
        {
            Type = type;
        }

        public abstract bool AddToFifo(CrdCfg crdCfg, short crd, short fifo, short cardId);
    }

    internal class M60Line : M60InterpolationMotionParamter
    {
        public Position Position { get; private set; }

        public M60Line(Position position) : base(M60InterpolationMotionParamterType.Line)
        {
            Position = position;
        }

        public override bool AddToFifo(CrdCfg crdCfg, short crd, short fifo, short cardId)
        {
            var axisIdArray = crdCfg.axis.Where(x => x > 0).ToArray();
            var pos = Position;
            var minMoveVel = double.MaxValue;
            var maxAccVel = 0D;
            var posArray = new int[8];
            for (int i = 0; i < axisIdArray.Length; i++)
            {
                // 获取导程
                var axisId = axisIdArray[i];
                var ratio = pos.AxisList.Single(x => x.AxisID == axisId).Ratio;

                // 计算位置
                var profile = pos.VelocityProfiles.Single(x => x.AxisId == axisId);
                var axisPos = profile.Pos * ratio;
                posArray[i] = (int)axisPos;

                // 最小运行速度和最大减速度
                var moveVel = pos.IsAllowUniform ? profile.MoveVel * (MachineDataManager.Instance.MotionSpeedPercentage / 100) * ratio
                                                 : profile.MoveVel * ratio;
                if (moveVel < minMoveVel)
                {
                    minMoveVel = moveVel;
                }
                var accVel = /*pos.IsAllowUniform ? profile.Acc * (MachineDataManager.Instance.MotionSpeedPercentage / 100) * ratio
                                               : */profile.Acc * ratio;
                if (accVel > maxAccVel)
                {
                    maxAccVel = accVel;
                }
            }

            // 插入直线点位
            var ret = ecat_motion.M_Line(crd, crdCfg.dimension, ref crdCfg.axis[0], ref posArray[0], minMoveVel, maxAccVel, 0, fifo, cardId);
            if (ret != 0)
            {
                throw new InvalidProgramException($"卡 {cardId} 插补坐标系 {crd} FIFO{fifo}# 压入点位 {Position.PositionName} 失败，返回值 {ret}。");
            }

            return true;
        }
    }

    internal class M60BufIos : M60InterpolationMotionParamter, IEnumerable<M60BufIOItem>
    {
        internal class M60BufIOItem
        {
            public OutputSetting Output { get; private set; }
            public bool IsHeightLevel { get; private set; }

            public M60BufIOItem(OutputSetting output, bool isHeightLevel)
            {
                Output = output;
                IsHeightLevel = isHeightLevel;
            }
        }

        private readonly List<M60BufIOItem> _m60BufIOItems = new List<M60BufIOItem>();

        public M60BufIos() : base(M60InterpolationMotionParamterType.BufIO)
        {
        }

        public M60BufIos(IEnumerable<M60BufIOItem> ioItems) : base(M60InterpolationMotionParamterType.BufIO)
        {
            _m60BufIOItems = new List<M60BufIOItem>(ioItems);
        }

        public IEnumerator<M60BufIOItem> GetEnumerator() => _m60BufIOItems.GetEnumerator();

        IEnumerator IEnumerable.GetEnumerator() => GetEnumerator();


        public override bool AddToFifo(CrdCfg crdCfg, short crd, short fifo, short cardId)
        {
            var ioGroup = _m60BufIOItems.Select(x => new
            {
                x.Output.ModuleName,
                Channel = (x.Output.Port - 1) % 16,
                ChannelBegin = x.Output.Port - (x.Output.Port % 16) + 1,
                Value = x.IsHeightLevel ? 1 << (x.Output.Port - 1) % 16 : 0,
                Mask = 1 << (x.Output.Port - 1) % 16
            })
         .GroupBy(x => (x.ModuleName, x.ChannelBegin))       // 按照IO模块和起始通道（16个一组）分组
         .Select(x => new
         {
             // 计算同组的IO模块和起始通道的
             x.Key,
             Value = x.Aggregate(0, (total, next) => (total | next.Value)),
             Mask = x.Aggregate(0, (total, next) => (total | next.Mask)),
         })
         .ToList();

            foreach (var item in ioGroup)
            {
                var ret = ecat_motion.M_BufMultiDO(crd, (short)item.Key.ModuleName, (short)item.Key.ChannelBegin, (short)item.Value, (short)item.Mask, fifo, cardId);
                if (ret != 0)
                {
                    throw new InvalidProgramException($"卡 {cardId} 插补坐标系 {crd} FIFO{fifo}# 压入IO {string.Join(",", _m60BufIOItems.Select(x => x.Output.Name))} 失败，返回值 {ret}。");
                }
            }

            return true;
        }


    }

    internal class M60Delay : M60InterpolationMotionParamter
    {
        public int delay { get; private set; }

        public M60Delay(int delay) : base(M60InterpolationMotionParamterType.Delay)
        {
            this.delay = delay;
        }

        public override bool AddToFifo(CrdCfg crdCfg, short crd, short fifo, short cardId)
        {
            var ret = ecat_motion.M_BufDelay(crd, (uint)delay, fifo, cardId);
           
            if (ret != 0)
            {
                throw new InvalidProgramException($"延时压入失败");
            }


            return true;
        }
    }
}
